/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/

#ifndef __FLEETFORMATIONWXGLFRAME_H__
#define __FLEETFORMATIONWXGLFRAME_H__

#include <wx/wx.h>
#include <wx/glcanvas.h>
#include "thread.hpp"

#include "MaCICtrlClient.hpp"
#include "SpeedCtrlClient.hpp"
#include "PositionClient.hpp"
#include "Component.hpp"

//class FleetFormationGUIMainFrame;
/*
GLCanvas to be used in Opengl-drawing
*/

#define CLEAR_COLOR         0.2,0.2,0.2

#define GRID_COLOR          0.5,0.5,0.5
#define GRID_LINE_WIDTH     1
#define AXIS_COLOR          0.5,0.0,0.0

#define MAP_COLOR           0.2,0.2,0.2
#define MAP_LINE_WIDTH      2

#define SCALE_MAX           300
#define SCALE_MIN           10

#define THROTTLE_PER_CENT_DELTA 5

//Forward declaration
class FleetFormationGUIMainFrame;


struct TViewParams
{
  //EDisplayType displayType;
  //ERenderMode renderMode;
  int Scale; //pixels per meter
  float Left, Right, Top, Bottom; //view bounds in 2d
  float OriginX, OriginY; //view center point in 2d
  int Width, Height; //view dimensions in pixels
  float parkAreaSize; //height of the park area
};

struct SRobotIcon{
  SRobotIcon(float aPositionX,float aPositionY):
    positionX(aPositionX),
    positionY(aPositionY),
    heading(0.0),
    iPositionClient(NULL),
    iSpeedCtrlClient(NULL){}
  

  /**
   *
   */
  void GetTriangleCorners(float &aX1,float &aY1,
                          float &aX2,float &aY2,
                          float &aX3,float &aY3){


    /*    float x1 = positionX,y1 = positionY-10;
    float x2 = positionX-10, y2= positionY+10;
    float x3 = positionX +10, y3 = positionY+10;
    */
    float x1 = 10,y1 = 0;
    float x2 =-10, y2=10;
    float x3 =-10, y3 =-10;
    float screenHeading = -heading;
    aX1 = positionX + x1*cos(screenHeading) -y1*sin(screenHeading);
    aY1 = positionY + x1*sin(screenHeading) +y1*cos(screenHeading);

    aX2 = positionX + x2*cos(screenHeading) -y2*sin(screenHeading);
    aY2 = positionY + x2*sin(screenHeading) +y2*cos(screenHeading);

    aX3 = positionX + x3*cos(screenHeading) -y3*sin(screenHeading);
    aY3 = positionY + x3*sin(screenHeading) +y3*cos(screenHeading);
   
    
  }
     
  bool IsInsideRobot(float aX,float aY){

    if(aX >=positionX -10 &&aX <=positionX +10 &&
       aY >= positionY-10 &&aY <=positionY+10){
      return true;
    }else{
      return false;
    }

  }                    
  void SetPosition(float aX, float aY){
    positionX = aX;
    positionY = aY;


  }
  int positionX;
  int positionY;
  float heading;
  MaCI::Position::CPositionClient *iPositionClient;
  MaCI::SpeedCtrl::CSpeedCtrlClient *iSpeedCtrlClient;


};
  
struct  SSlaveMachine{

  SSlaveMachine(MaCI::Position::CPositionClient *aPositionClient ,
                MaCI::SpeedCtrl::CSpeedCtrlClient *aSpeedCtrlClient,
                MaCI::Position::TPose2D aRelativePosition )
    :positionClient(aPositionClient),
     speedCtrlClient(aSpeedCtrlClient),
     relativePosition(aRelativePosition){


  }
  
  MaCI::Position::CPositionClient *positionClient;
  MaCI::SpeedCtrl::CSpeedCtrlClient *speedCtrlClient;
  MaCI::Position::TPose2D relativePosition;

};

class FleetFormationGUIMainFrame;
class CFleetFormationWXGLFrame : public wxGLCanvas,
                                 private gim::CSync,
                                 private gim::CThread
                                
{
  DECLARE_EVENT_TABLE()
  public:
    CFleetFormationWXGLFrame(wxWindow* parent, FleetFormationGUIMainFrame* mainFrame);

    ~CFleetFormationWXGLFrame();

  void SetRobots(  std::map<std::string, SRobotIcon*> *aRobotMap);
  std::map<std::string, SRobotIcon*>::iterator GetMasterRobot();
  void GetSlaveRobots(std::vector<SSlaveMachine> *aSlaveVector,
                      bool aRelativeToMasterHeading = false);

  void ShowRealPose(bool aState);

  void DrawRelativeToMasterHeading(bool aState);

  /**
   * "Locks" screen so that the formation can't be edited
   */
  void LockGLScreen();
  /**
   * Unlocks screen so that the formation can be edited
   */
  void UnlockGLScreen();

protected:
    void OnPaint(wxPaintEvent &e);
    void OnSize(wxSizeEvent &e);
    void OnErase(wxEraseEvent &e);
    void OnLeftDown(wxMouseEvent &e);
    void OnLeftUp(wxMouseEvent &e);
    void OnDoubleClick(wxMouseEvent &e);
    void OnRightDown(wxMouseEvent &e);
    void OnRightUp(wxMouseEvent &e);
    void OnMotion(wxMouseEvent &e);
    void OnMouseWheel(wxMouseEvent &e);
    void OnLeaveWindow(wxMouseEvent &e);
    void OnChar(wxKeyEvent &e);
    //void OnContextMenu(wxContextMenuEvent &e);
    
    
    void SetGrid(bool grid);
    
    
    static int attribList[];

    const TViewParams& GetViewParams();
    
 
    
  private:

  FleetFormationGUIMainFrame *mainFrame;
    
    
  wxGLContext GLContext;

  /**2d view params**/
  TViewParams iView;  
  bool followSelected;
  std::map<std::string, SRobotIcon*> *iRobotMap;
  std::map<std::string, SRobotIcon*>::iterator iMasterRobotIter;
  std::map<std::string, SRobotIcon*>::iterator iSelectedRobotIterator;
  std::map<std::string,SRobotIcon *>::iterator iAttachedRobotIterator;
  bool iIsRobotAttached;
  // float iMasterHeading;  
  bool grid;
  float gridZ;
  float gridSize;

  bool iShowRealPose;

  bool iScreenLocked;
  bool iDrawRelativeToMasterHeading;
  int ThreadFunction(const int aThreadNumber);
  void Draw();
  void DrawGrid(); 
  void DrawRobots();   
  void SetView();
  //function for drawing text
  void drawText(float x, float y, float z, void *font, const char *string);
};



#endif
